/*
 * Copyright 2018 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_

#include <map>
#include <set>
#include <vector>

#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/submaps.h"

namespace cartographer
{
    namespace mapping
    {

        // The current state of the submap in the background threads. After this
        // transitions to 'kFinished', all nodes are tried to match
        // against this submap. Likewise, all new nodes are matched against submaps in
        // that state.
        enum class SubmapState
        {
            kNoConstraintSearch,
            kFinished
        };

        struct InternalTrajectoryState
        {
            enum class DeletionState
            {
                NORMAL,
                SCHEDULED_FOR_DELETION,
                WAIT_FOR_DELETION
            };

            PoseGraphInterface::TrajectoryState state = PoseGraphInterface::TrajectoryState::ACTIVE;
            DeletionState deletion_state = DeletionState::NORMAL;
        };

        struct InternalSubmapData
        {
            std::shared_ptr<const Submap> submap;

            SubmapState state = SubmapState::kNoConstraintSearch;

            // IDs of the nodes that were inserted into this map together with
            // constraints for them. They are not to be matched again when this submap
            // becomes 'kFinished'.
            std::set<NodeId> node_ids;
        };

        struct PoseGraphData
        {
            // Submaps get assigned an ID and state as soon as they are seen, even
            // before they take part in the background computations.
            // InternalSubmapData包含子图数据(前端子图对象)，子图状态，其内的nodeid集合
            MapById<SubmapId, InternalSubmapData> submap_data;
            // submap的全局位姿
            //  Global submap poses currently used for displaying data.
            MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
            MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;

            // Data that are currently being shown.
            // 一帧点云表示一个节点，TrajectoryNode包含前端的数据和全局位姿
            MapById<NodeId, TrajectoryNode> trajectory_nodes;

            // Global landmark poses with all observations.
            // key is landmark ID
            std::map<std::string, PoseGraphInterface::LandmarkNode> landmark_nodes;
            // 轨迹间的关联时间
            TrajectoryConnectivityState trajectory_connectivity_state;
            int num_trajectory_nodes = 0;
            // 每个轨迹状态
            std::map<int, InternalTrajectoryState> trajectories_state;
            // 每个轨迹的初始位姿
            // Set of all initial trajectory poses.
            std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

            std::vector<PoseGraphInterface::Constraint> constraints;
        };

    } // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_
